#include "Stdmain.h"
#include "Form.h"
#include "Bounds.h"
#include <fstream>

const char* g1file = "/home/mjarvis/bhuv_fake/gamma1srclzs1unp90all.data";
const char* g2file = "/home/mjarvis/bhuv_fake/gamma12srclzs1unp90all.data";
const char* kfile = "/home/mjarvis/bhuv_fake/kappasrclzs1unp90all.data";

// Constants to set:
const double minsep = 15.;       // (arcsec) Minimum separation to consider
const double maxsep = 35.*60.;  // (arcsec) Maximum separation to consider
const double binsize = 0.2;    // Ratio of distances for consecutive bins

const double scale = 200.*60./2048.;  // conversion of Bhuv's pix to arcsec
  
const double logbinratio = binsize;
const double binratio = exp(logbinratio);
const int nbins = int(ceil(log(maxsep/minsep)/logbinratio));

struct DECFloat {
  float val;

  void Read(istream& fin)
  {
    char bytes[4];
    fin.read(bytes,4);
    //SWAP(bytes[0],bytes[3]);
    //SWAP(bytes[1],bytes[2]);
    val = *(reinterpret_cast<float*>(bytes));
  }
};
			
ifstream& operator>>(ifstream& fin, DECFloat& x)
{ x.Read(fin); return fin; }

void Add(double x,double& sx,double& vx)
{
  if (!(x == x)) {
    cerr<<"x = "<<x<<endl;
  } else {
    sx += x;
    vx += x*x;
  }
}

void Normalize(double& sx,double& vx,double n)
{
  dbg<<"In Norm: nap = "<<n<<"  "<<sx<<","<<vx<<" --> ";
  if (n<2) {
    sx = vx = 0.;
  } else {
    sx /= n;
    vx -= n*sx*sx;
    if (vx < 0.) {
      cerr<<"error: vx = "<<vx<<", init: vx = "<<(vx+n*sx*sx)<<", sx = "<<(sx*n)<<", n = "<<n<<endl;
    }
    vx /= n*(n-1);
  }
  dbg<<sx<<","<<vx<<endl;
}

double Q(double x)
{
  double xsq = x*x;
  return xsq*exp(-xsq/2.);
}

void GetMap(const vector<vector<DComplex> >& garray,
    const DComplex& apc, double apr,
    double* retmap, double* retmx, 
    double* retgt2, double* retgtx, double* retgx2,
    double* retgt3, double* retgt2x, double* retgtx2, double* retgx3)
{
  // position for shape i,j is (i,j)*180.*3600./2048. (arcsec)

  // calculate 1/(4 Pi R^2) int dxdy (r/R)^2 exp(-(r/R)^2/2) g_t(x,y)
  // = 1/(4 Pi R^2) int dxdy Q(r/R) g_t(x,y)
  // Q dies off pretty quickly, so only integrate for r/R < 4
  
  double map=0.;  // For now these are summing the integral without the 
  double mx=0.;   // 1/(4 Pi R^2) dxdy factors
  double sgt2=0., sgx2=0.;

  int imin = int(ceil((apc.real()-4.*apr)/scale));
  if (imin < 0) imin = 0;
  int imax = int(floor((apc.real()+4.*apr)/scale));
  if (imax > 2047) imax = 2047;
  //dbg << "apc = "<<apc<<", apr = "<<apr<<", imin = "<<imin<<", imax = "<<imax<<endl;
  double x = imin*scale-apc.real();
  //dbg << "x = "<<x<<", 4*apr = "<<4.*apr<<endl;
  for(int i=imin; i<=imax; i++,x+=scale) {
    //dbg << "x = "<<x<<", 4*apr = "<<4.*apr<<endl;
    double dysq = SQR(4.*apr)-SQR(x);
    //dbg << "dysq = "<<dysq<<endl;
    Assert(dysq >= 0.);
    double dy = sqrt(dysq);
    int jmin = int(ceil((apc.imag()-dy)/scale));
    if (jmin < 0) jmin = 0;
    int jmax = int(floor((apc.imag()+dy)/scale));
    if (jmax > 2047) jmax= 2047;
    //dbg<<"jmin, jmax = "<<jmin<<','<<jmax<<endl;
    double y = jmin*scale-apc.imag();
    for(int j=jmin; j<=jmax; j++,y+=scale) {
      //dbg <<"y = "<<y<<endl;
      DComplex direc = DComplex(x,y);
      double r = fabs(direc);
      if (r==0) continue; // q = 0, so contrib to map,mx = 0
      direc /= r;
      double q = Q(r/apr);
      DComplex g = garray[i][j];
      g *= SQR(conj(direc));
      g *= q;
      double gt = -g.real();
      double gx = g.imag();
      map += gt;
      mx += gx;
      double gtsq = gt;
      double gxsq = gx;
      sgt2 += gtsq;
      sgx2 += gxsq;
    }
  }

  double temp = scale*scale / 4./ PI / SQR(apr);
  map *= temp;
  mx *= temp;
  double temp2 = temp*temp;
  sgt2 *= temp2;
  sgx2 *= temp2;

  *retmap = map;
  *retmx = mx;
  *retgt2 = sgt2;
  *retgx2 = sgx2;
}

int main()
{
  dbgout = &cout;

  ifstream g1in(g1file,ios::binary);
  ifstream g2in(g2file,ios::binary);

  DECFloat g1, g2;

  g1in >> g1;
  g2in >> g2;

  Form sci8; sci8.sci().prec(8).trail(1);

  vector<vector<DComplex> > garray(2048,vector<DComplex>(2048));

  Bounds b;

  for(uint i=0;i<2048;i++) for(uint j=0;j<2048;j++) {
    // start at top left
    // j is distance right
    // i is distance up
    g1in >> g1;
    g2in >> g2;
    garray[j][i] = DComplex(g1.val,g2.val);
    b += DComplex(j,i)*scale;
  }

  if (!g1in) myerror("reading g1in");
  if (!g2in) myerror("reading g2in");

  static Form sci; sci.sci().prec(2).width(9).trail(1); 
  static Form fix; fix.fix().prec(3).width(9).trail(1);

  int i1ar[] = {0,0,0,10,0,0,11,11};
  int j1ar[] = {0,10,0,0,0,11,11,0};
  int i2ar[] = {12,12,0,10,11,3,0,8};
  int j2ar[] = {0,10,12,12,3,0,8,11};
  int i3ar[] = {6,6,10,0,3,11,8,0};
  int j3ar[] = {10,0,6,6,11,8,0,3};
  double sumepepep=0.,sumepepex=0.,sumepexex=0.,sumexexex=0.;
  int n = 0;
  for(int i=0;i<2000;i++) for(int j=0;j<2000;j++) {
    for(int tri=0;tri<8;tri++) {
      int i1 = i+i1ar[tri];
      int i2 = i+i2ar[tri];
      int i3 = i+i3ar[tri];
      int j1 = j+j1ar[tri];
      int j2 = j+j2ar[tri];
      int j3 = j+j3ar[tri];
      DComplex z1(i1,j1); z1 *= scale;
      DComplex z2(i2,j2); z2 *= scale;
      DComplex z3(i3,j3); z3 *= scale;
      DComplex e1 = garray[i1][j1];
      DComplex e2 = garray[i2][j2];
      DComplex e3 = garray[i3][j3];
      DComplex centroid = (z1+z2+z3)/3.;
      DComplex q1 = centroid - z1;
      DComplex q2 = centroid - z2;
      DComplex q3 = centroid - z3;
      //cout <<"z = "<<z1<<','<<z2<<','<<z3<<endl;
      //cout <<"d = "<<fabs(z2-z3)<<','<<fabs(z1-z3)<<','<<fabs(z1-z2)<<endl;
      //cout <<"q = "<<q1<<','<<q2<<','<<q3<<endl;
      //cout <<"e = "<<e1<<','<<e2<<','<<e3<<endl;
      e1 *= SQR(conj(q1))/norm(q1);
      e2 *= SQR(conj(q2))/norm(q2);
      e3 *= SQR(conj(q3))/norm(q3);
      //cout <<"-> e = "<<e1<<','<<e2<<','<<e3<<endl;
      sumepepep += e1.real()*e2.real()*e3.real();
      sumepepex += e1.imag()*e2.real()*e3.real();
      sumepepex += e1.real()*e2.imag()*e3.real();
      sumepepex += e1.real()*e2.real()*e3.imag();
      sumepexex += e1.imag()*e2.imag()*e3.real();
      sumepexex += e1.imag()*e2.real()*e3.imag();
      sumepexex += e1.real()*e2.imag()*e3.imag();
      sumexexex += e1.imag()*e2.imag()*e3.imag();
      //cout <<"sum +++ = "<<sumepepep<<", ++x = "<<sumepepex<<", +xx = "<<sumepexex<<", xxx = "<<sumexexex<<endl;
    }
  }
  cout <<"sum +++ = "<<sumepepep<<", ++x = "<<sumepepex<<", +xx = "<<sumepexex<<", xxx = "<<sumexexex<<endl;

  ofstream fout("simplem.out");

  fout <<"#  R(')  .  <Map>  .  sig_m  .  <Mx>   .  sig_mx . <Map^2> . sig_m^2 .  <MMx>  . sig_mmx .  <Mx^2> . sig_mx^2.  <M^3>  .  sig_m3 . <M^2Mx> .sig_m^2mx. <MMx^2> .sig_mmx^2. <Mx^3>  . sigmx^3 .  nap    \n";
  for(int bin=0;bin<nbins;bin++) {
    double apr = exp(bin*logbinratio)*minsep;
    dbg<<"bin = "<<bin<<", apr = "<<apr<<endl;

    double sm=0.,smx=0.,sm2=0.,smmx=0.,smx2=0.,sm3=0.,sm2mx=0.,smmx2=0.,smx3=0.;
    double vm=0.,vmx=0.,vm2=0.,vmmx=0.,vmx2=0.,vm3=0.,vm2mx=0.,vmmx2=0.,vmx3=0.;
    int nap=0;

    for(double apx = b.GetXMin()+2.*apr; apx < b.GetXMax()-2.*apr; apx += 0.5*apr) {
      for(double apy = b.GetYMin()+2.*apr; apy < b.GetYMax()-2.*apr; apy += 0.5*apr) {
	DComplex apc(apx,apy);
	// dbg <<"ap # "<<nap<<", apc = "<<apc<<endl;
	double m,mx,sgt2,sgx2;
	GetMap(garray,apc,apr,&m,&mx,&sgt2,&sgx2);
	double dm,dmx,dsgt2,dsgx2;
	GetMap(garray,apc,2*apr,&dm,&dmx,&dsgt2,&dsgx2);
	// dbg <<"m = "<<m<<", mx = "<<mx<<endl;
	nap++;
	double msq = m*m;
	double mxsq = mx*mx;
	Add(m,sm,vm);
	Add(mx,smx,vmx);
	Add(msq-sgt2,sm2,vm2);
	Add(m*mx,smmx,vmmx);
	Add(mxsq-sgx2,smx2,vmx2);
	Add(dm*msq,sm3,vm3);
	Add(dmx*msq,sm2mx,vm2mx);
	Add(dm*mxsq,smmx2,vmmx2);
	Add(dmx*mxsq,smx3,vmx3);
      }
    }
    Normalize(sm,vm,nap);
    Normalize(smx,vmx,nap);
    Normalize(sm2,vm2,nap);
    Normalize(smmx,vmmx,nap);
    Normalize(smx2,vmx2,nap);
    Normalize(sm3,vm3,nap);
    Normalize(sm2mx,vm2mx,nap);
    Normalize(smmx2,vmmx2,nap);
    Normalize(smx3,vmx3,nap);

    dbg <<"done: ave m = "<<sm<<" +- "<<sqrt(vm)<<", m2 = "<<sm2<<" +- "<<sqrt(vm2)<<endl;
    fout << fix(apr/60.);
    fout << sci(sm) << sci(sqrt(vm));
    fout << sci(smx) << sci(sqrt(vmx));
    fout << sci(sm2) << sci(sqrt(vm2));
    fout << sci(smmx) << sci(sqrt(vmmx));
    fout << sci(smx2) << sci(sqrt(vmx2));
    fout << sci(sm3) << sci(sqrt(vm3));
    fout << sci(sm2mx) << sci(sqrt(vm2mx));
    fout << sci(smmx2) << sci(sqrt(vmmx2));
    fout << sci(smx3) << sci(sqrt(vmx3));
    fout << nap << endl;
  }

  if (dbgout && dbgout != &cout) {delete dbgout; dbgout=0;}
  return 0;
}
 
